#include "bukekedishidrive.h"

BukeKedishiDrive::BukeKedishiDrive( QObject* parent ) {
    time = 50;

#ifdef NJLAKCAN
    canDevice = new NJLikeCanDevice();
#endif
#ifdef ZLGCAN
    canDevice = new ZLGCanDevice();
#endif

    Zero_Pulse                       = -2407666;
    pulse_per                        = 131072;  //电机一圈脉冲数值
    Veer_Wheel_Speed_Reduction_Radio = 64.0;
    pos_mode_speed                   = 1000;  //舵轮转角 rmp
    pos_mode_acc                     = 500;   // rps^2
    pos_mode_dec                     = 500;   // rps^2

    Walk_Wheel_R                     = 0.125;
    Walk_Wheel_Speed_Reduction_Radio = 21;
}

bool BukeKedishiDrive::CanRecviveData() {
    if ( !is_open_can ) { return false; }
    CAN_DataFrame vco[ 200 ];
    ULONG         dwRel = CAN_ChannelReceive( dw_DeviceHandle, dw_Channel, vco, 200, 40 );
    if ( dwRel <= 0 ) { return false; }
    for ( ULONG i = 0; i < dwRel; i++ ) {
        QByteArray recv;
        for ( int k = 0; k < vco[ i ].nDataLen; k++ ) {
            recv.append( static_cast< char >( vco[ i ].arryData[ k ] ) );
        }
        emit sigShowCanData( recv, "Recv:" + QString::number( vco[ i ].uID, 16 ) );
        /***
         * 柯蒂斯直行电机（科蒂斯0x180+Tra-ID(0x26)）
         * [0][1]:电机转速，0-8000rpm/min对应100-8000+100；
         * [5]:油泵故障码(0x12~0x99(HEX)
         * [6]:行走故障码(0x12~0x99(HEX)
         * [7]:转向故障码(0x12~0x99(HEX)
         */
        if ( vco[ i ].uID == 0X1a6 ) {
            // rpm
            short int recive_walk_speed = static_cast< short int >( ( vco[ i ].arryData[ 0 ] ) + ( vco[ i ].arryData[ 1 ] << 8 ) );
            // m/s
            fork_walk_reality_speed = static_cast< double >( recive_walk_speed ) / Walk_Wheel_Speed_Reduction_Radio / 60 * ( 2 * M_PI * Walk_Wheel_R );
            if ( ( vco[ i ].arryData[ 5 ] != 0 ) || ( vco[ i ].arryData[ 6 ] != 0 ) || ( vco[ i ].arryData[ 7 ] != 0 ) ) { emit sigSendTips( "can叉车故障！" ); }
        }
        /***
         * 前进后退升降反馈（科蒂斯0x280+Tra-ID(0x26)）
         * [2][3]:数值为-12000~+12000,
         * -12000左转+12000右转,
         * 对应（-120deg～120deg）
         * [0]:Bit0	前进方向反馈
         *     Bit1	后退方向反馈
         *     Bit2	起升反馈
         *     Bit3	下降反馈
         *     Bit4	当前模式状态反馈：ON：手动，OFF：自动
         *     Bit5	前移反馈（无效）
         *     Bit6	后移反馈（无效）
         *     Bit7	Interlock状态(ON：自动模式)
         */
        else if ( vco[ i ].uID == 0x2a6 ) {
            if ( static_cast< short int >( vco[ i ].arryData[ 0 ] ) == 0x90 ) {}
        }
        /***
         * [0][1]:电机温度，–100 – 300°C对应–1000 – 3000，
         * [2][3]:控制器温度，–100 – 300°C对应–1000 – 3000，
         * [4][5]:加速度电压, 0.00 – 5.00 V对应0 – 32767
         * [6][7]：转向指令电压, 0.00 – 5.00 V对应0 – 32767
         */
        else if ( vco[ i ].uID == 0x3a6 ) {
        }
        // 步科 舵轮脉冲反馈
        //        else if ( vco[ i ].uID == static_cast< unsigned int >( 0x580 + turn_drive_id ) ) {
        //            if ( ( vco[ i ].nDataLen == 8 ) && ( vco[ i ].arryData[ 0 ] == 0x43 ) && ( vco[ i ].arryData[ 1 ] == 0x64 ) && ( vco[ i ].arryData[ 2 ] == 0x60 ) && ( vco[ i ].arryData[ 3 ] ==
        //            0x00 ) ) {
        //                int recive_veer_puls = ( vco[ i ].arryData[ 4 ] ) + ( vco[ i ].arryData[ 5 ] << 8 ) + ( vco[ i ].arryData[ 6 ] << 16 ) + ( vco[ i ].arryData[ 7 ] << 24 );
        //                //（脉冲差值/一圈脉冲数）*2π /减速比
        //                fork_veer_reality_rad = -static_cast< double >( recive_veer_puls - Zero_Pulse ) / pulse_per * ( 2 * M_PI ) / Veer_Wheel_Speed_Reduction_Radio;
        //            }
        //        }
    }
    return true;
}

void BukeKedishiDrive::RegularGetData() {

    if ( !is_open_can || !is_auto_model ) { return; }

    ULONG        i = 0;
    CanDataFrame frames[ 20 ];

    /**柯蒂斯直行电机***********************************/
    //设置直行
    /***
     * [0]:
     *  Bit0	前进：1有效，0无效
     *  Bit1	后退：1有效，0无效
     *  Bit2	自动模式Interlock:1有效，0无效
     *  Bit3	BRAKE刹车：1有效，0无效
     *  Bit4
     *  Bit5
     *  Bit6
     *  Bit7
     * [1][2] 速度 0-8000对应100-8000+100rpm/min
     * [3] Accel Rate行走电机加速率 1~255对应0.1s~25.5s
     * [4] Decel Rate行走电机减速率 1~255对应0.1s~25.5s
     * [5] Max Speed Accel行走电机加速率 1~255对应0.1s~25.5s
     * [6] Max Speed Decel行走电机减速率 1~255对应0.1s~25.5s
     * [7] 刹车强度 0~255对应0~100%
     */
    kds_speed_byte[ 0 ] = kds_speed_state;
    kds_speed_byte[ 1 ] = static_cast< BYTE >( kds_speed );
    kds_speed_byte[ 2 ] = static_cast< BYTE >( kds_speed >> 8 );
    kds_speed_byte[ 3 ] = 0x01;
    kds_speed_byte[ 4 ] = 0x01;
    kds_speed_byte[ 5 ] = 0x01;
    kds_speed_byte[ 6 ] = 0x01;
    kds_speed_byte[ 7 ] = 0xFF;
    CanDataFrameConfig( frames[ i++ ], 0x226, kds_speed_byte );
    /***
     * [0]:升（1），降（2）
     *    Bit0	起升使能：1有效，0无效
     *    Bit1	下降使能：1有效，0无效
     *    Bit2
     *    Bit3
     *    Bit4
     *    Bit5
     *    Bit6
     *    Bit7
     * [1]:0
     * [2]:0
     * [3] 起升速度 255
     * [4][5] 下降速度 32767
     * [6]:0
     * [7]:0
     */
    //设置升降
    kds_fork_up_donw_byte[ 0 ] = kds_fork_up_down_state;
    kds_fork_up_donw_byte[ 3 ] = fork_lft_speed;
    kds_fork_up_donw_byte[ 4 ] = kds_updown_speed[ 0 ];
    kds_fork_up_donw_byte[ 5 ] = kds_updown_speed[ 1 ];
    CanDataFrameConfig( frames[ i++ ], 0x326, kds_fork_up_donw_byte );
    /**柯蒂斯直行电机***********************************/

    /***********步科 设置旋转角***************************/
    /**设置位置*/
    step_speed_set[ 4 ] = static_cast< BYTE >( fork_calc_puls );
    step_speed_set[ 5 ] = static_cast< BYTE >( fork_calc_puls >> 8 );
    step_speed_set[ 6 ] = static_cast< BYTE >( fork_calc_puls >> 16 );
    step_speed_set[ 7 ] = static_cast< BYTE >( fork_calc_puls >> 24 );
    CanDataFrameConfig( frames[ i++ ], 0x601, step_speed_set );

    /***********步科 设置旋转角**************************/

    CanSendData( frames, i );
}

void BukeKedishiDrive::CanSetSpeedAngle( double walk_speed, double veer_rad ) {

    // 速度m/s = 给定速度（Walk_Wheel_Speed）* 60 / 周长（Walk_Wheel_C） *
    // 减速比（Walk_Wheel_Speed_Reduction_Radio）
    if ( walk_speed < 0 ) {
        //后退使能
        kds_speed_state = 0x06;
    }
    else {
        //前进使能
        kds_speed_state = 0x05;
    }
    walk_speed              = fabs( walk_speed );
    fork_walk_setting_speed = walk_speed;
    fork_veer_setting_rad   = veer_rad;
    kds_speed               = static_cast< short >( ( fork_walk_setting_speed * 60 / Walk_Wheel_C ) * Walk_Wheel_Speed_Reduction_Radio );
    // 步科 脉冲值 = 弧度
    fork_calc_puls = static_cast< long >( -fork_veer_setting_rad * Veer_Wheel_Speed_Reduction_Radio * pulse_per / ( 2 * M_PI ) + Zero_Pulse );
}

void BukeKedishiDrive::push_fork_upper() {
    kds_updown_speed[ 0 ]  = 0x00;
    kds_updown_speed[ 1 ]  = 0x00;
    fork_lft_speed         = 0xFF;
    kds_fork_up_down_state = 0x01;
}

void BukeKedishiDrive::push_fork_stop() {
    kds_updown_speed[ 0 ]  = 0x00;
    kds_updown_speed[ 1 ]  = 0x00;
    fork_lft_speed         = 0x00;
    kds_fork_up_down_state = 0x00;
}

void BukeKedishiDrive::push_fork_down() {
    kds_updown_speed[ 0 ]  = 0xFF;
    kds_updown_speed[ 1 ]  = 0x7F;
    fork_lft_speed         = 0x00;
    kds_fork_up_down_state = 0x02;
}

/**
 * @brief BukeKedishiDrive::switchAutoModel
 * 步可旋转电机自动模式配置
 * 1.去掉手动模式配置
 * 2.位置模式配置
 */
void BukeKedishiDrive::switchAutoModel() {
    if ( is_auto_model || !is_open_can ) { return; }
    autoModelInit();
    is_auto_model = true;
}

void BukeKedishiDrive::switchHandleModel() {
    if ( !is_auto_model || !is_open_can ) { return; }
    handleModelInit();
    is_auto_model = false;
}

void BukeKedishiDrive::sltForkLiftDrive_Init() {
    if ( CanOpenAndStart() ) {  // open can drive
        emit sigSendTips( "DmakeMotecDrive 正确，CAN设备打开成功" );
    }
    else {
        emit sigSendTips( "DmakeMotecDrive 错误，CAN设备打开失败" );
        return;
    }
    if ( is_auto_model ) { autoModelInit(); }
    else {
        handleModelInit();
    }
    emit sigSendTips( "提示，电机初始化完成！" );
    Utility::delayMs( 1000 );  // 非阻塞延时执行
    QTimer* timer50ms = new QTimer();
    connect( timer50ms, SIGNAL( timeout() ), this, SLOT( sltTimeOut() ) );
    timer50ms->start( time );
}

void BukeKedishiDrive::autoModelInit() {
    ULONG        i = 0;
    CanDataFrame frames[ 20 ];
    /***************步科==转向轮设置自动模式*******************/
    step_DIN1_set[ 5 ] = 0x00;
    step_DIN2_set[ 5 ] = 0x00;
    step_DIN5_set[ 5 ] = 0x00;
    step_DIN6_set[ 5 ] = 0x00;
    //重置手动模式下 DIN配置，不然位置模式不期效果
    CanDataFrameConfig( frames[ i++ ], 0x601, step_DIN1_set );
    CanDataFrameConfig( frames[ i++ ], 0x601, step_DIN2_set );
    CanDataFrameConfig( frames[ i++ ], 0x601, step_DIN5_set );
    CanDataFrameConfig( frames[ i++ ], 0x601, step_DIN6_set );
    /***************步科==转向轮设置自动模式*******************/
    /******步科==转向轮设置位置模式*******/
    /**1.断电*/
    step_control_set[ 4 ] = 0x06;
    step_control_set[ 5 ] = 0x00;
    CanDataFrameConfig( frames[ i++ ], 0x601, step_control_set );
    /**2.上电*/
    step_control_set[ 4 ] = 0x0F;
    step_control_set[ 5 ] = 0x00;
    CanDataFrameConfig( frames[ i++ ], 0x601, step_control_set );
    /**3.位置模式*/
    step_model_set[ 4 ] = 0x01;
    step_model_set[ 5 ] = 0x00;
    CanDataFrameConfig( frames[ i++ ], 0x601, step_model_set );
    /**4.设置速度*/
    long calc_speed     = ( ( pos_mode_speed * 10 ) / 60 ) * pulse_per;
    step_speed_set[ 4 ] = static_cast< BYTE >( calc_speed );
    step_speed_set[ 5 ] = static_cast< BYTE >( calc_speed >> 8 );
    step_speed_set[ 6 ] = static_cast< BYTE >( calc_speed >> 16 );
    step_speed_set[ 7 ] = static_cast< BYTE >( calc_speed >> 24 );
    CanDataFrameConfig( frames[ i++ ], 0x601, step_speed_set );
    /**5.long m_acce加速度     使用默认**/
    CanDataFrameConfig( frames[ i++ ], 0x601, step_speed_set );
    /**6.long m_dece减速度     使用默认**/
    CanDataFrameConfig( frames[ i++ ], 0x601, step_speed_set );
    /**7.103F控制字  ***/
    CanDataFrameConfig( frames[ i++ ], 0x601, step_control_set );
    /**8.设置位置 **/
    CanDataFrameConfig( frames[ i++ ], 0x601, step_control_set );
    /******步科==转向轮设置位置模式*******/
    CanSendData( frames, i );
}

void BukeKedishiDrive::handleModelInit() {
    ULONG        i = 0;
    CanDataFrame frames[ 20 ];

    // 重置手动模式下 DIN配置，不然位置模式不期效果
    step_DIN1_set[ 5 ] = 0x04;
    step_DIN2_set[ 5 ] = 0x08;
    step_DIN5_set[ 5 ] = 0x01;
    step_DIN6_set[ 5 ] = 0x04;

    CanDataFrameConfig( frames[ i++ ], 0x601, step_DIN1_set );
    CanDataFrameConfig( frames[ i++ ], 0x601, step_DIN2_set );
    CanDataFrameConfig( frames[ i++ ], 0x601, step_DIN5_set );
    CanDataFrameConfig( frames[ i++ ], 0x601, step_DIN6_set );
    // 断电06
    step_control_set[ 4 ] = 0x00;
    step_control_set[ 5 ] = 0x06;
    CanDataFrameConfig( frames[ i++ ], 0x601, step_control_set );
    // 上电0F
    step_control_set[ 4 ] = 0x00;
    step_control_set[ 5 ] = 0x0F;
    CanDataFrameConfig( frames[ i++ ], 0x601, step_control_set );
    // DIN模式
    step_model_set[ 4 ] = 0x01;
    CanDataFrameConfig( frames[ i++ ], 0x601, step_control_set );
    // 102F控制字
    step_control_set[ 4 ] = 0x10;
    step_control_set[ 5 ] = 0x2F;
    CanDataFrameConfig( frames[ i++ ], 0x601, step_control_set );

    CanSendData( frames, i );
}
